/*
 * File - ros_objectdetection_publisher.cpp
 *
 * This file is part of the Inuitive SDK
 *
 * Copyright (C) 2014-2021 Inuitive All Rights Reserved
 *
 */

/**
 * \cond INTERNAL
 */

#include "config.h"

#include <iostream>
#include <nodelet/nodelet.h>
#include <ros/rate.h>
#include <ros/ros.h>
#include <sensor_msgs/distortion_models.h>
#include <vision_msgs/Detection2DArray.h>

#include <opencv2/opencv.hpp>

/**
 * \endcond
 */

#include "ros_objectdetection_publisher.h"

#include "inudev_ros_nodelet.h"

/**
 * \file ros_objectdetection_publisher.cpp
 *
 * \brief CRosObjectDetectionPublisher
 */

/**
 * \cond INTERNAL
 */

static const char rcsid[]="$Id: 0180d9d763423b7363cb29501c53ae5e998f31b4 $";

/**
 * \endcond
 */

namespace __INUROS__NAMESPACE__
{
    CRosObjectDetectionPublisher::CRosObjectDetectionPublisher(ros::NodeHandle& _node,  std::shared_ptr<image_transport::ImageTransport> _image_transport, CRosSensor* _rosSensor, std::string iTopicName)
            : CRosPublisher(_node, _image_transport, "vision_msgs", _rosSensor)
            , publisher(node.advertise<vision_msgs::Detection2DArray>(iTopicName, 1, false))
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());
        mObjects.header.stamp = ros::Time::now();
    }

    void CRosObjectDetectionPublisher::FrameCallback(std::shared_ptr<InuDev::CCnnAppStream> iStream, std::shared_ptr<const InuDev::CCnnAppFrame> iFrame, InuDev::CInuError iError)
    {
        NODELET_DEBUG_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        std::shared_ptr<std::vector<InuDev::CCnnAppFrame::CDetectedObject>> tmpObjects = iFrame->GetObjectData();
        
        if (!mObjects.detections.empty())
        {
            mObjects.detections.clear();
        }
        
        if (tmpObjects)
        {
            for (std::vector<InuDev::CCnnAppFrame::CDetectedObject>::iterator it = tmpObjects->begin(); it != tmpObjects->end(); ++it)
            {
                //NODELET_WARN_STREAM("Found Object: " + it->ClassID);
                vision_msgs::Detection2D newObject;
                newObject.header.stamp = ros::Time::now();
                newObject.header.frame_id = it->ClassID;
                newObject.header.seq = iFrame->FrameIndex;
                newObject.bbox.center.x = it->ClosedRectTopLeft.X() + (it->ClosedRectSize.X() / 2);
                newObject.bbox.center.y = it->ClosedRectTopLeft.Y() + (it->ClosedRectSize.Y() / 2);
                newObject.bbox.size_x = it->ClosedRectSize.X();
                newObject.bbox.size_y = it->ClosedRectSize.Y();
                mObjects.detections.push_back(newObject);
            }
        }

        publisher.publish(mObjects);
    }

    InuDev::CInuError CRosObjectDetectionPublisher::RegisterCallback()
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        InuDev::CCnnAppStream::CallbackFunction callback = std::bind(&CRosObjectDetectionPublisher::FrameCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);

        return std::static_pointer_cast<InuDev::CCnnAppStream>(stream)->Register(callback);
    }

    InuDev::CInuError CRosObjectDetectionPublisher::UnregisterCallback()
    {
        return std::static_pointer_cast<InuDev::CCnnAppStream>(stream)->Register(nullptr);
    }

    int CRosObjectDetectionPublisher::GetNumSubscribers()
    {
        publisherPrev = publisherCurr;
        publisherCurr = publisher.getNumSubscribers();

        return publisherCurr;
    }

    InuDev::CInuError CRosObjectDetectionPublisher::InitStream()
    {
        NODELET_INFO_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName());

        InuDev::CCnnAppFrame::EOutputType ObjectDetectionType = InuDev::CCnnAppFrame::eObjectDetection;
        std::string ObjectDetectionTypetmp;
        ros::param::param<std::string>("ObjectDetectionType", ObjectDetectionTypetmp, "SSD");

        if (ObjectDetectionTypetmp == "YOLOV3")
        {
            ObjectDetectionType = InuDev::CCnnAppFrame::eObjectDetectionYoloV3;
        }

        InuDev::CInuError err = std::static_pointer_cast<InuDev::CCnnAppStream>(stream)->Init(ObjectDetectionType);

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        return InuDev::EErrorCode::eOK;
    }

    InuDev::CInuError CRosObjectDetectionPublisher::StartStream()
    {
        stream = std::static_pointer_cast<InuDev::CBaseStream>(sensor->getSensor()->CreateCnnAppStream());
        NODELET_ERROR_STREAM_COND(!stream, __INUROS_FUNCTION_NAME__ << ": failed creating " << getName());

        InuDev::EErrorCode err = InitStream();

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        err = std::static_pointer_cast<InuDev::CCnnAppStream>(stream)->Start();

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": " << getName() << " 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        if (!mObjects.detections.empty())
        {
            mObjects.detections.clear();
        }

        err = RegisterCallback(); // generic!

        if (err != InuDev::EErrorCode::eOK)
        {
            NODELET_ERROR_STREAM(__INUROS_FUNCTION_NAME__ << ": Register callback: 0x" << std::hex << std::setw(8) << std::setfill('0') << int(err));
            return err;
        }

        return InuDev::EErrorCode::eOK;
    }

}
